//
// Created by cch on 2022/5/1.
//

#ifndef IMU_GPS_EKF_INITALIZER_H
#define IMU_GPS_EKF_INITALIZER_H

#include "base_type.h"
#include <deque>
#include <utility>

namespace IMUGPS {
class Initializer {
public:
  Initializer(Eigen::Vector3d init_I_P_GPS):init_I_P_GPS_(std::move(init_I_P_GPS)){};
public:
  void AddImuData(const ImuDataPtr &imuDataPtr);

  bool AddGpsData(const GpsDataPtr& gpsDataPtr,State *state );
  bool ComputeRotationFromImu(const Eigen::Matrix3d &G_R_I);

private:
  const Eigen::Vector3d init_I_P_GPS_;
  std::deque<ImuDataPtr> imu_buffer;


};
} // namespace IMUGPS

#endif // IMU_GPS_EKF_INITALIZER_H
